#include "vis/paint.h"

#include <ctime>
#include <random>

#define WIDTH 1600
#define HEIGHT 800
#define RANGE_X 100
#define RANGE_Y 50

Paint::Paint() : width_(WIDTH), height_(HEIGHT) {
  background_mat_ = Mat::zeros(height_, width_, CV_8UC3);
  flush_mat_ = Mat::zeros(height_, width_, CV_8UC3);
}

Paint::~Paint() {}

void Paint::Draw(const autodrive::perception::Map& map) {
  flush_mat_.copyTo(background_mat_);

  std::uint32_t marking_count = map.road_marking.size();
  std::default_random_engine e;
  std::uniform_int_distribution<int> u(0, 255);  // 左闭右闭区间
  e.seed(time(0));
  // draw points
  for (int i = 0; i < marking_count; i++) {
    int r = u(e);
    int g = u(e);
    int b = u(e);
    Scalar sca(b, g, r);
    const autodrive::perception::RoadMarking& marking = map.road_marking.at(i);

    std::uint32_t point_count = marking.points.size();
    for (int j = 0; j < point_count; ++j) {
      const autodrive::perception::MapElementPoint& point =
          marking.points.at(j);

      circle(background_mat_, vehicle2paint(point.x, point.y), 1, sca, 1,
             LINE_AA, 0);
    }
  }

  // draw polygen

#if 0
  for (int i = 0; i < map.output_roadmarking.GetNumberFilled(); ++i) {
    int r = u(e);
    int g = u(e);
    int b = u(e);
    // Scalar sca(r, g, b);
    Scalar sca(0, 255, 0);

    std::vector<cv::Point> pts;
    const autodrive::perception::RoadMarkingResult& result =
        map.output_roadmarking.Get(i);

    pts.push_back(
        vehicle2paint(result.polygon.Get(0).x(), result.polygon.Get(0).y()));
    pts.push_back(
        vehicle2paint(result.polygon.Get(1).x(), result.polygon.Get(1).y()));
    pts.push_back(
        vehicle2paint(result.polygon.Get(2).x(), result.polygon.Get(2).y()));
    pts.push_back(
        vehicle2paint(result.polygon.Get(3).x(), result.polygon.Get(3).y()));
    polylines(background_mat_, pts, true, sca, 1, 8, 0);

    putText(background_mat_, "id = " + std::to_string(result.id),
            vehicle2paint(result.polygon.Get(0).x(), result.polygon.Get(0).y()),
            cv::FONT_HERSHEY_PLAIN, 1.0, sca);
  }

#endif

  background(background_mat_);

  Show();
}

void Paint::Draw(const std::vector<std::vector<Eigen::Vector2f>>& points) {
  flush_mat_.copyTo(background_mat_);
  int count = points.size();
  std::default_random_engine e;
  std::uniform_int_distribution<int> u(0, 255);  // 左闭右闭区间
  e.seed(time(0));
  for (int i = 0; i < count; i++) {
    int r = u(e);
    int g = u(e);
    int b = u(e);

    const std::vector<Eigen::Vector2f>& vec = points.at(i);
    // Scalar sca(0 + 255 / count * i, 255 - 255 / count * i,
    //            255 - 255 / count * i);
    Scalar sca(r, g, b);
    for (int j = 0; j < vec.size(); j++) {
      const Eigen::Vector2f point = vec.at(j);
      circle(background_mat_, vehicle2paint(point.x(), point.y()), 1, sca, 1,
             LINE_AA, 0);
    }
  }

  background(background_mat_);
  Show();
}

void Paint::Draw(const std::vector<std::vector<Eigen::Vector2f>>& points,
                 std::vector<cv::Mat>& mats) {
  flush_mat_.copyTo(background_mat_);
  int count = points.size();
  std::default_random_engine e;
  std::uniform_int_distribution<int> u(0, 255);  // 左闭右闭区间
  e.seed(time(0));
  for (int i = 0; i < count; i++) {
    int r = u(e);
    int g = u(e);
    int b = u(e);
    Scalar sca(r, g, b);

    const std::vector<Eigen::Vector2f>& vec = points.at(i);
    for (int j = 0; j < vec.size(); j++) {
      const Eigen::Vector2f point = vec.at(j);
      circle(background_mat_, vehicle2paint(point.x(), point.y()), 1, sca, 1,
             LINE_AA, 0);
    }
  }

  for (int i = 0; i < mats.size(); ++i) {
    int r = u(e);
    int g = u(e);
    int b = u(e);
    // Scalar sca(r, g, b);
    Scalar sca(0, 255, 0);

    std::vector<cv::Point> pts;
    const cv::Mat& m = mats.at(i);

    std::cout << "draw mat = " << i << " " << mats[i] << std::endl;

    pts.push_back(vehicle2paint(m.at<float>(0, 0), m.at<float>(0, 1)));
    pts.push_back(vehicle2paint(m.at<float>(1, 0), m.at<float>(1, 1)));
    pts.push_back(vehicle2paint(m.at<float>(2, 0), m.at<float>(2, 1)));
    pts.push_back(vehicle2paint(m.at<float>(3, 0), m.at<float>(3, 1)));

    polylines(background_mat_, pts, true, sca, 1, 8, 0);
  }

  background(background_mat_);

  Show();
}

void Paint::Draw(std::vector<Eigen::Vector2f>& origin_points,
                 const std::vector<std::vector<Eigen::Vector2f>>& points,
                 std::vector<cv::Mat>& mats) {
  flush_mat_.copyTo(background_mat_);
  int count = points.size();
  std::default_random_engine e;
  std::uniform_int_distribution<int> u(0, 255);  // 左闭右闭区间
  e.seed(time(0));
  for (int i = 0; i < count; i++) {
    int r = u(e);
    int g = u(e);
    int b = u(e);
    Scalar sca(g, b, r);

    const std::vector<Eigen::Vector2f>& vec = points.at(i);
    for (int j = 0; j < vec.size(); j++) {
      const Eigen::Vector2f point = vec.at(j);
      circle(background_mat_, vehicle2paint(point.x(), point.y()), 1, sca, 1,
             LINE_AA, 0);
    }
  }

  for (int i = 0; i < mats.size(); ++i) {
    int r = u(e);
    int g = u(e);
    int b = u(e);
    Scalar sca(r, g, b);

    std::vector<cv::Point> pts;
    const cv::Mat& m = mats.at(i);

    pts.push_back(vehicle2paint(m.at<float>(0, 0), m.at<float>(0, 1)));
    pts.push_back(vehicle2paint(m.at<float>(1, 0), m.at<float>(1, 1)));
    pts.push_back(vehicle2paint(m.at<float>(2, 0), m.at<float>(2, 1)));
    pts.push_back(vehicle2paint(m.at<float>(3, 0), m.at<float>(3, 1)));

    polylines(background_mat_, pts, true, sca, 1, 8, 0);
  }

  count = origin_points.size();
  for (int i = 0; i < count; i++) {
    Scalar sca(0, 0, 255);
    const Eigen::Vector2f& point = origin_points.at(i);
    std::cout << "draw : " << point.x() << " " << point.y() << std::endl;
    circle(background_mat_, vehicle2paint(point.x(), point.y()), 1, sca, 5,
           LINE_AA, 0);
  }

  background(background_mat_);

  Show();
}

void Paint::background(cv::Mat& mat) {
  Scalar red(0, 0, 255);
  Scalar white(255, 255, 255);
  int thickness = 1;
  float range = 10.0f;
  for (int i = 0; i < RANGE_X / range; i++) {
    if (i == 0) {
      float x = 0.0f, y = float(RANGE_Y);
      line(mat, vehicle2paint(x, y), vehicle2paint(x, -y), red, thickness,
           LINE_AA, 0);
    } else {
      float x1 = range * i, y1 = float(RANGE_Y);
      float x2 = -range * i, y2 = float(RANGE_Y);
      line(mat, vehicle2paint(x1, y1), vehicle2paint(x1, -y1), white, thickness,
           LINE_AA, 0);
      line(mat, vehicle2paint(x2, y2), vehicle2paint(x2, -y2), white, thickness,
           LINE_AA, 0);
    }
  }

  for (int i = 0; i < RANGE_Y / range; i++) {
    if (i == 0) {
      float x = float(RANGE_X), y = 0.0f;
      line(mat, vehicle2paint(x, y), vehicle2paint(-x, y), red, thickness,
           LINE_AA, 0);
    } else {
      float x1 = float(RANGE_X), y1 = range * i;
      float x2 = float(RANGE_X), y2 = -range * i;
      line(mat, vehicle2paint(x1, y1), vehicle2paint(-x1, y1), white, thickness,
           LINE_AA, 0);
      line(mat, vehicle2paint(x2, y2), vehicle2paint(-x2, y2), white, thickness,
           LINE_AA, 0);
    }
  }

  putText(mat, std::to_string(-(int)RANGE_X), Point(10, 10), 2, 0.5,
          Scalar(155, 155, 155));
  putText(mat, std::to_string((int)RANGE_X), Point(WIDTH - 30, 10), 2, 0.5,
          Scalar(155, 155, 155));

  putText(mat, std::to_string((int)RANGE_Y), Point(5, 20), 2, 0.5,
          Scalar(155, 155, 155));
  putText(mat, std::to_string(-(int)RANGE_Y), Point(5, HEIGHT - 10), 2, 0.5,
          Scalar(155, 155, 155));
}

void Paint::Show() {
  background(background_mat_);
  imshow("结果", background_mat_);
  cv::waitKey(500);
}

Point Paint::vehicle2paint(float x, float y) {
  Point p;
  p.x = (float)(WIDTH / RANGE_X / 2) * (x + float(RANGE_X));
  p.y = (float)(HEIGHT / RANGE_Y / 2) * (float(RANGE_Y) - y);
  return p;
}
